/**
 * @file exploring_maze.cpp
 * @author 范文宇 (2643660853@qq.com)
 * @brief
 * @version 1.0
 * @date 2024-01-06
 *
 * @copyright Copyright (c) 2024
 *
 */
#include <ctime>
#include <random>
#include <cstdlib>
#include "exploring_maze.h"

using namespace std;

RandomGoal::RandomGoal()
{
    // 初始化
    treahutstate_sub = nh.subscribe<std_msgs::Int8>("/exploring_cmd", 5, &RandomGoal::treahutstateCallBack, this);
    position_sub = nh.subscribe<nav_msgs::Odometry>("/odom", 5, &RandomGoal::positionCallBack, this);
    voice_pub = nh.advertise<std_msgs::String>("/voiceWords", 1000);
    // 定位current_goal的父坐标
    current_goal.target_pose.header.frame_id = "map";
    // 定寻宝藏机器人的目标朝向，让寻宝机器人可以更高概率的转圈，摄像头容易找到拍摄到宝藏
    current_goal.target_pose.pose.orientation.w = 1;
}

void RandomGoal::positionCallBack(nav_msgs::Odometry odom_msg)
{
    // 更新机器人位姿
    odom_data = odom_msg;
}

void RandomGoal::treahutstateCallBack(std_msgs::Int8 treasure_state_msg)
{
    // 更新宝藏状态
    treasure_state = treasure_state_msg;
}

int main(int argc, char **argv)
{
    srand(static_cast<unsigned>(time(0)));
    ros::init(argc, argv, "exploring_maze");
    MoveBaseClient ac("move_base", true);
    // 开启服务通信
    RandomGoal a;
    ROS_INFO("Waiting for the move_base action serve");
    ac.waitForServer(ros::Duration(60));
    while (1)
    {
        // 回调函数，读取宝藏状态和机器人本身位置
        ros::spinOnce();
        // 如果未发现宝藏
        if (a.treasure_state.data == 0)
        {
            std_msgs::String msg;
            msg.data = "你好";
            a.voice_pub.publish(msg);
            a.current_goal.target_pose.header.stamp = ros::Time::now();
            // 起始点，为了保证第一次随机目标点不在原点，限制了第一次目标点的设置
            if (fabs(a.odom_data.pose.pose.position.x) <= 0.2 &&
                fabs(a.odom_data.pose.pose.position.y) <= 0.2)
            {
                a.current_goal.target_pose.pose.position.x = -1.0 + static_cast<float>(rand()) / (static_cast<float>(RAND_MAX / 2));
                a.current_goal.target_pose.pose.position.y = static_cast<float>(rand()) / (static_cast<float>(RAND_MAX / 6));
            }
            // 到达一个目标点后，更新目标点
            if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
            {
                std::default_random_engine e1(time(0));
                std::normal_distribution<double> distribution1(4.09, 1.0);
                double rnd_x = distribution1(e1);
                std::default_random_engine e2(time(0));
                std::normal_distribution<double> distribution2(7.74, 1.0);
                double rnd_y = distribution2(e2);
                a.current_goal.target_pose.pose.position.x = rnd_x;
                a.current_goal.target_pose.pose.position.y = rnd_y;
            }
            ac.sendGoal(a.current_goal);
            ac.waitForResult();
        }
        // 倘若找到了宝藏，直接回到起始点
        else if (a.treasure_state.data == 1)
        {
            move_base_msgs::MoveBaseGoal final_goal;
            final_goal.target_pose.pose.position.x = 0;
            final_goal.target_pose.pose.position.y = 0;
            final_goal.target_pose.header.frame_id = "map";
            final_goal.target_pose.pose.orientation.w = 1;
            ac.sendGoal(final_goal);
        }
    }

    return 0;
}